class PIDController:
    def __init__(self, kp=1.0, ki=0.0, kd=0.0):
        """
        _summary_
        :param kp: _description_, defaults to 1.0
        :param ki: _description_, defaults to 0.0
        :param kd: _description_, defaults to 0.0
        """
        self.kp = kp
        self.ki = ki
        self.kd = kd
        
        self.previous_error = 0.0
        self.integral = 0.0
    
    def compute(self, setpoint: float, measured_value: float, dt: float):
        """
        计算PID控制输出
        :param setpoint: 设定值
        :param measured_value: 测量值
        :param dt: 时间步长(秒)
        :return: _description_
        """
        error = setpoint - measured_value
        # 比例项
        p_term = self.kp * error
        # 积分项
        self.integral += error * dt
        i_term = self.ki * self.integral
        # 微分项
        derivative = (error - self.previous_error) / dt
        d_term = self.kd * derivative
        # 计算控制输出
        output = p_term + i_term + d_term
        # 保存当前误差用于下一次计算
        self.previous_error = error
        return output
    

if __name__ == '__main__':
    pid = PIDController()
    pass
